/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.defaultCode;


import edu.wpi.first.wpilibj.Counter;
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.SmartDashboard;
import edu.wpi.first.wpilibj.Watchdog;
import java.util.Date;
import java.util.Timer;

public class DefaultRobot extends IterativeRobot
{
    DriverStation driverStation;

    DriveTrain dt;
    Timer runTimer;
    Controller controller;
    boolean enabled = false;
//    Counter flRrSteerCnt;
//    DigitalInput inputA;
//    DigitalInput inputB;

    public DefaultRobot()
    {
        driverStation = DriverStation.getInstance();
        controller = new Controller();
        runTimer = new Timer();
//        inputA = new DigitalInput(4, 5);
//        inputB = new DigitalInput(4, 6);
//        flRrSteerCnt = new Counter(EncodingType.k4X, inputA, inputB, false);

    }

    /********************************** Init Routines *************************************/

	public void robotInit()
        {
      //      controller.initialize();
	}

	public void disabledInit() {

	}

	public void autonomousInit()
        {
       //     controller.initialize();
	}

	public void teleopInit()
        {

	}

        public void enable()
        {

            if(!enabled)
            {
             controller.initialize();
          //  runTimer.schedule(controller, new Date(), 10);
            enabled = true;
//                   flRrSteerCnt.start();
            }
        }

	public void disabledPeriodic()
        {
		Watchdog.getInstance().feed();
	}

	public void autonomousPeriodic()
        {
		// feed the user watchdog at every period when in autonomous
		Watchdog.getInstance().feed();
	}


	public void teleopPeriodic()
        {
//            SmartDashboard.log(flRrSteerCnt.get(), "COUNTER");
            Watchdog.getInstance().feed();
            enable();
            controller.run();
	}
}
